#include <termios.h>
#include <rclcpp/rclcpp.hpp>
#include "sensor_cpp_pkg/msg/sensor_data.hpp"
#include "../include/sensor_cpp_pkg/modbus_rtu.hpp"

using namespace std::chrono_literals;

class SensorPublisher : public rclcpp::Node {
public:
    SensorPublisher() : Node("sensor_hcd5014_publisher") {
        // 声明参数
        this->declare_parameter<std::string>("serial_port", "/dev/ttyUSB0");
        this->declare_parameter<int>("baud_rate", 9600);
        this->declare_parameter<int>("device_address", 0x33);
        this->declare_parameter<int>("retry_interval", 5); // 重试间隔(秒)
        this->declare_parameter<int>("read_interval", 1);  // 读取间隔(秒)
        this->declare_parameter<std::string>("robot_id","robot1001");
        
        // 获取参数
        this->get_parameter("serial_port", serial_port_);
        this->get_parameter("baud_rate", baud_rate_);
        this->get_parameter("device_address", device_address_);
        this->get_parameter("retry_interval", retry_interval_);
        this->get_parameter("read_interval", read_interval_);
        this->get_parameter("robot_id", robot_id_);
        
        publisher_ = this->create_publisher<sensor_cpp_pkg::msg::SensorData>("sensor_hcd5014_data", 10);
        
        // 初始化连接
        initializeConnection();
        
        // 创建定时器处理重连和数据读取
        timer_ = this->create_wall_timer(
            std::chrono::seconds(read_interval_),
            [this]() { this->timer_callback(); });
    }

private:
    void initializeConnection() {
        // 将波特率转换为termios常量
        speed_t baud_rate_constant = getBaudRateConstant(baud_rate_);
        if (baud_rate_constant == 0) {
            RCLCPP_ERROR(this->get_logger(), "不支持的波特率: %d", baud_rate_);
            connection_status_ = -1; // 不支持的波特率
            return;
        }
        
        // 创建或重新创建Modbus连接
        modbus_ = std::make_unique<ModbusRTU>(serial_port_.c_str(), baud_rate_constant, device_address_);
        if (!modbus_->connect()) {
            RCLCPP_WARN(this->get_logger(), "无法连接到 Modbus 设备 %s", serial_port_.c_str());
            connection_status_ = 0; // 连接失败
            return;
        }
        
        RCLCPP_INFO(this->get_logger(), "成功连接到 Modbus 设备 %s", serial_port_.c_str());
        connection_status_ = 1; // 连接成功
        retry_count_ = 0;
    }
    
    void timer_callback() {
        // 如果没有连接成功，尝试重连
        if (connection_status_ != 1) {
            retry_count_++;
            if (retry_count_ % retry_interval_ == 0) { // 每隔retry_interval秒尝试重连一次
                RCLCPP_INFO(this->get_logger(), "尝试重新连接到 Modbus 设备 (%d 次尝试)", retry_count_);
                initializeConnection();
            }
            return;
        }
        
        // 已连接，读取数据
        std::vector<uint16_t> values;
        if (!modbus_->readHoldingRegisters(0x0001, 6, values)) {
            RCLCPP_WARN(this->get_logger(), "读取传感器数据失败");
            // 可以在这里添加连接状态检查逻辑
            return;
        }

        if (values.size() != 6) {
            RCLCPP_WARN(this->get_logger(), "读取到的数据长度不正确");
            return;
        }

        constexpr float SCALE_100 = 100.0f;
        constexpr float SCALE_10 = 10.0f;

        auto msg = sensor_cpp_pkg::msg::SensorData();
        msg.robot_id = robot_id_;
        msg.air_temperature = static_cast<float>(values[0]) / SCALE_100 - 40.0f;
        msg.air_humidity = static_cast<float>(values[1]) / SCALE_100;
        msg.light_intensity = static_cast<float>(values[2]) * SCALE_10;
        msg.co2_concentration = values[3];
        msg.soil_temperature = static_cast<float>(values[4]) / SCALE_100 - 20.0f;
        msg.soil_moisture = static_cast<float>(values[5]) / 10.0f;

        publisher_->publish(msg);

        RCLCPP_DEBUG(this->get_logger(),
                    "\n设备Id: %s\n"
                    "空气温度: %.2f ℃\n"
                    "空气湿度: %.2f %%\n"
                    "光照强度: %.2f Lux\n"
                    "CO2浓度: %u ppm\n"
                    "土壤温度: %.2f ℃\n"
                    "土壤湿度: %.1f %%",
                    msg.robot_id.c_str(),
                    msg.air_temperature, msg.air_humidity, msg.light_intensity,
                    msg.co2_concentration, msg.soil_temperature, msg.soil_moisture);
    }
    
    speed_t getBaudRateConstant(int baud_rate) {
        switch (baud_rate) {
            case 9600:  return B9600;
            case 19200: return B19200;
            case 38400: return B38400;
            case 57600: return B57600;
            case 115200:return B115200;
            default:    return 0;
        }
    }

    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Publisher<sensor_cpp_pkg::msg::SensorData>::SharedPtr publisher_;
    std::unique_ptr<ModbusRTU> modbus_;
    
    // 参数变量
    std::string serial_port_;   // 串口设备路径
    int baud_rate_;             // 波特率
    int device_address_;        // 设备地址
    int retry_interval_;        // 重连间隔(秒)
    int read_interval_;         // 读取间隔(秒)
    std::string robot_id_;      // 添加robot_id变量
    
    // 连接状态管理
    int connection_status_ = 0; // 0=未连接, 1=已连接, -1=错误配置
    int retry_count_ = 0;
};

int main(int argc, char* argv[]) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<SensorPublisher>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}